clear all;
clc
x=[31.788264
    8.2046468
    24.932131
    31.385926
    34.193719
    14.415668
    4.015959
    -6.366519
    56.83676
    1.366547
    2.330773
    2.871039
    3.394591
    3.97096
    4.96349];
x=[ 39.466290000000001
    8.562912000000001
    19.094860000000001
    47.838859999999997
    13.385560000000000
    12.211961000000001
    6.201630000000000
    29.722500000000000
    23.454499999999999
    6.119370000000000
    0.193040000000000
    0.440830000000000
    0.684670000000000
    0.958350000000000
    1.355330000000000];
% ga
x=[ 35.02074
    6.404196
    31.60722
    50.59949
    20.80324
    41.54364
    60
    18.07791
    0
    6.283185
    0.264935
    0.500377
    0.735321
    0.996529
    1.333549];
x=[26.3045770732468;8.68786829594409;17.2215923743877;31.1724659571717;23.8293741151157;4.37472282583319;3.81089310598608;47.5988734279221;3.83684366670517;1.80507692195434;2.49639736441707;3.04437578590416;3.61303989109469;4.24295915823125;5.98028039892871];
x=[22.7008953043986;6.30288250735659;19.0333046781791;1.39240434020063;-28.8919043757334;-27.0366730806382;53.2221089427493;46.7321440791420;5.74775162755239;5.35041370920869;5.72703317296679;6.02277096641236;6.28296041241328;0.245091476503090;0.569217512173087]
% WY Lin1 and KM Hsiao: A new differential evolution algorithm with a combined mutation strategy for optimum synthesis of path-generating four-bar mechanisms
% x=[45.9540268793200;10.1413352283990;36.1198616002200;49.7905570636800;51.4935231082160;16.4957302836340;59.9251894962240;-0.267255177889410;0.862384954187540;4.49683139849550;3.94685292206840;3.51208539098020;3.11078124880880;2.71010931195190;2.26549866257340];
% x=[18.1304774597420;8.51190563603640;50.5261110390030;47.4165113621710;51.6932129228460;-59.9706964849970;-50.6632020069740;44.5248269164710;5.80155035478320;2.21032595635080;2.50154874474090;2.78997173731280;3.07713085604920;3.36378097604160;3.65145100974960];
Cdx=[20 20 20 20 20 20];
Cdy=[20 25 30 35 40 45];
r1=x(1);
r2=x(2);
r3=x(3);
r4=x(4);
rcx=x(5);
rcy=x(6);
error=0;
for i=1:length(Cdx)
    % teta3
    teta2=x(9+i);
    K1=r1/r2;
    K4=r1/r3;
    K5=(r4^2-r1^2-r2^2-r3^2)/(2*r3*r2);
    D=cos(teta2)-K1+K4*cos(teta2)+K5;
    E=-2*sin(teta2);
    F=K1+(K4-1)*cos(teta2)+K5;
    ATAN3=(-E-sqrt(E^2-4*D*F))/(2*D);
    teta3=real(2*atan(ATAN3));
    Crx=r2*cos(teta2)+rcx*cos(teta3)-rcy*sin(teta3);
    Cry=r2*sin(teta2)+rcx*sin(teta3)+rcy*cos(teta3);
    C(:,i)=[cos(x(9)),-sin(x(9));sin(x(9)),cos(x(9))]*[Crx;Cry]+[x(7);x(8)];
    error=error+sum((C(:,i)-[Cdx(i);Cdy(i)]).^2);
end
error

